﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ExtKalmanFilter.cs
 * \brief	This file contains the implementation of the Extended Kalman Filter.
 *  
 * This file contains the implementation of the Extended Kalman Filter to fussion the
 * data coming from the Odometer service and the INS-GPS Sensor.
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.02
 * \date      2010-06-08
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio Matta / Jaime del Cerro 2008-06-10 - 2008-07-10 \n
 *      Creation of the basic Navigation Sensor Fusion for the NM-RS project
 *      
 * - Version 0.02: Bjoern STEURER                           2009-05-15 \n
 *   minor changes due to implementation of new interfaces struture
 *   
 * - Version 0.03: Antonio MATTA                            2010-03-11 \n
 *   minor documentation added.
 *   
 * - Version 0.04: Antonio MATTA                            2010-06-08 \n
 *   Plot method for velocity component added.
 */
/* ######################################################################### */

/*Autogenerated*/
using System;
using System.Collections.Generic;
using System.Security.Permissions;
using System.ComponentModel;
using System.Net;
using System.Linq;
using System.Text;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using of input output library.*/
using System.IO;

/*Using the Globalization library.*/
using System.Globalization;

/* ----------------------------------------------------------------------------------- */
/**	\namespace SenerUpm_SharedClasses
 *
 *  \brief This is the namespace of the classes to be shared among different modules. 
 */
/* ------------------------------------------------------------------------------------ */
namespace NMRS.M44_SharedClasses
{
    /* ------------------------------------------------------------------------- */
    /**	\class ExtKalmanFilter
     *
     *  \brief  This is the class where the Extended Kalman Filter properties and
     *  methods are encapsuled. All these properties and methods are used to 
     *  fussion the data coming from the Odometer service and the INS-GPS Sensor.
     *
     */
    /* ------------------------------------------------------------------------- */
    public class ExtKalmanFilter
    {
        #region Initialization of the extended kalman filter public variables
        /* ------------------------------------------------------------------------- */
        /**	\struct KalmanOutput
         * 
         *  \brief It defines the Kalman Filter output data structure.
         *  
         *  In this structure, three matrices are stored which define the Extended Kalman
         *  Filter output data.
         *
         */
        /* ------------------------------------------------------------------------- */
        public struct KalmanOutput
        {
            /* ##### INPUT VARIABLES ##### */

            /*! OUTPUT: tt_EstPos. Estimated position by the kalman filter */
            public Matrix tt_EstPos;
            /*! OUTPUT: tt_K. Kalman Gain */
            public Matrix tt_K;
            /*! OUTPUT: tt_P. Covarianze matrix */
            public Matrix tt_P;


            public KalmanOutput( Matrix X,
                                 Matrix K,
                                 Matrix P
                                )
            {
                tt_EstPos = X;
                tt_K = K;
                tt_P = P;
            }
        }
        
        public Matrix _Xest = new Matrix(3, 1);     /*It stores the estimated position value*/
        public Matrix _XodoLast = new Matrix(3, 1);     /*It stores the estimated position value*/
        public Matrix _Xk = new Matrix(3, 1);       /*It stores the estimated position value at K time*/
        public Matrix _Pest = new Matrix(3, 3);     /*It stores the estimated Covarianze Matrix*/
        public Matrix _Pk = new Matrix(3, 3);       /*It stores the estimated Covarianze Matrix at K time*/
        public Matrix _Pk_1 = new Matrix(3, 3);     /*It stores the estimated Covarianze Matrix at (K-1) time*/
        public Matrix _Qk_1 = new Matrix(3, 3);     /*It stores the process noise covariance Q matrix*/
        public Matrix _R = new Matrix(3, 3);        /*It stores the measurement noise covariance R matrix*/
        public Matrix _Sk = new Matrix(3, 3);       /*Temporal Matrix during the process*/
        public Matrix _Kk = new Matrix(3, 3);       /*It stores the Kalman Gain matrix*/
        public Matrix _Ak = Matrix.Identity(3);     /*It stores the Jacobian A matrix at time K*/
        public Matrix _Wk = Matrix.Identity(3);     /*It stores the Jacobian W matrix at time K*/
        public Matrix _Hk = Matrix.Identity(3);     /*It stores the Jacobian H matrix at time K*/
        #endregion

        /* ------------------------------------------------------------------------- */
        /**	\fn public void ExtKalmanFilter()
         *
         *  \brief  Constructor, nothing needs to be here.
         *
         */
        /* ------------------------------------------------------------------------- */
        public ExtKalmanFilter() {} 

        /* ------------------------------------------------------------------------- */
        /**	\fn public ExtKalmanFilter( Matrix P, Matrix PCQ )
         *
         *  \brief  Constructor, with an initial value for _Pest and _Qk_1.
         *  
         *  \param[in] P    -> Initial Covariance Matrix.
         *  
         *  \param[in] PCQ  -> Initial Process Noise Covariance Matrix.
         *
         */
        /* ------------------------------------------------------------------------- */
        public ExtKalmanFilter( Matrix P, Matrix Q ) {

            /* Extended Kalman Filter initial values for the Covariance Matrix (P), 
             * and the Process Noise Covariance Matrix (PCQ)*/

            /*Assigment of these values to the proper matrices*/
            _Pest = P;
            _Qk_1 = Q;
            
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public void SetMatrixR( Matrix R )
         *
         *  \brief  It stores the Measurement Noise Covariance Matrix.
         *  
         *  \param[in] R  -> Initial Measurement Noise Covariance Matrix.
         *
         */
        /* ------------------------------------------------------------------------- */
        public void SetMatrixR( Matrix R )
        {
            /*Assigment of this value to the proper matrix*/
            _R = R;

        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public void SetXestIni( Matrix Xest )
         *
         *  \brief  It sets the initial value for the Xest matrix.
         *  
         *  \param[in] Xest  -> Matrix with the initial estimated values.
         *
         */
        /* ------------------------------------------------------------------------- */
        public void SetXestIni( Matrix Xest )
        {
            Matrix PosDiff = new Matrix(new double[] { 0.0001, 0.0001, 0.0001 });
            _Xest = Xest;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public KalmanOutput Update( Matrix Xodo, Matrix Zk )
         *
         *  \brief  This function implements the Extended Kalman Filter Algorithm.
         * 
         * \param[in] Xodo  -> Matrix with the odometer position
         * \param[in] Zk    -> Measurement matrix.It contains the GPS-INS values.
         * 
         * \return It returns Xest double vector containing the estimated robot position.
         *
         */
        /* ------------------------------------------------------------------------- */
        public KalmanOutput Update( Matrix Xodo, Matrix Zk )
        {
            
            /*State prediction*/
     
            _Pk_1 = _Pest;
            
            /*Odometry noise*/
            _Xk = _Xest + Xodo;

            _Pk = _Ak * _Pk_1 * _Ak.Transpose() + _Wk * _Qk_1 * _Wk.Transpose();
            
            /*Measurement update*/

            /*Compute Kalman Gain*/
            _Sk = _Hk * _Pk * _Hk.Transpose() + _R;
            _Kk = _Pk * _Hk.Transpose() * _Sk.Inverse();

            /*Update estimated pos with measurement Zk*/
            _Xest = _Xk + _Kk * (Zk - _Xk);
            _Pest = (Matrix.Identity(3) - _Kk * _Hk) * _Pk;
           
            
            /*Kalman output structure to store the 
             *Kalman's filter output data*/
            KalmanOutput r_KalmanOutput = new KalmanOutput( _Xest,
                                                            _Kk.DiagVector(),
                                                            _Pest.DiagVector()

                                              );

            return r_KalmanOutput;
            
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public void CreatePlotFile( float time, Matrix xreal, Matrix xsens, Matrix xest, Matrix K, Matrix P )
         *
         *  \brief It generates a data file to plot the real and estimated position  values
         *          using MatLab.
         *  
         */
        /* ------------------------------------------------------------------------- */
        public void CreatePosPlotFile( string RobotId, float time, Matrix xreal, Matrix xodo, 
                                    Matrix xsens, Matrix xest, long GPSQuality)
        {
            
            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            //new string(_state.RobotData.tc_RobotID) + "_INS-GPS";
            /*It creates filter.m file and stores the desired values*/
            
            using (StreamWriter w = File.AppendText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\"+ RobotId + ".m"))
            {
                w.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6}\t{7}\t{8}\t{9}\t{10}\t{11}\t{12}\t{13}",
                            time.ToString("N", nfi),
                            double.Parse(xreal[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xreal[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xreal[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[3, 1].ToString()).ToString("N", nfi),
                            GPSQuality.ToString("N", nfi)
                            );

                /*Update the underlying file.*/
                w.Flush();

                /*Close the writer and underlying file.*/
                w.Close();
            }
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public void CreatePlotFile( float time, Matrix xreal, Matrix xsens, Matrix xest, Matrix K, Matrix P )
         *
         *  \brief It generates a data file to plot the real and estimated position  values
         *          using MatLab.
         *  
         */
        /* ------------------------------------------------------------------------- */
        public void CreateVelPlotFile(string RobotId, float time, Matrix xreal, Matrix xodo,
                                    Matrix xsens, Matrix xest, Matrix K, Matrix P, long GPSQuality)
        {

            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            //new string(_state.RobotData.tc_RobotID) + "_INS-GPS";
            /*It creates filter.m file and stores the desired values*/

            using (StreamWriter w = File.AppendText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\" + RobotId + "-Vel.m"))
            {
                w.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6}\t{7}\t{8}\t{9}\t{10}\t{11}\t{12}\t{13}\t{14}\t{15}\t{16}\t{17}\t{18}\t{19}",
                            time.ToString("N", nfi),
                            double.Parse(xreal[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xreal[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xreal[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xodo[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xsens[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(xest[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(K[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(K[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(K[3, 1].ToString()).ToString("N", nfi),
                            double.Parse(P[1, 1].ToString()).ToString("N", nfi),
                            double.Parse(P[2, 1].ToString()).ToString("N", nfi),
                            double.Parse(P[3, 1].ToString()).ToString("N", nfi),
                            GPSQuality.ToString("N", nfi)
                            );

                /*Update the underlying file.*/
                w.Flush();

                /*Close the writer and underlying file.*/
                w.Close();
            }
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
